//std include
#include <string>
#include <vector>
#include <stdio.h>
#include <unordered_map>
#include <unordered_set>
#include <chrono>

#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Byte.h>
#include <rm_msgs/MoveJ.h>
#include <rm_msgs/Plan_State.h>
#include <rm_msgs/Arm_Current_State.h>
#include <rm_msgs/ArmState.h>
#include <rm_msgs/write_single_register.h>
#include <rm_msgs/set_modbus_mode.h>

class RealmanRobotClient
{
  public:
    ros::NodeHandle nh;
    ros::Publisher moveJ_pub;
    ros::Publisher clear_pub;
    ros::Publisher write_single_register_pub;
    ros::Publisher toolpower_pub;
    ros::Publisher set_modbus_pub;
    void init(ros::NodeHandle handle)
    {
        nh = handle;
        // 空间规划指令Publisher
        moveJ_pub = nh.advertise<rm_msgs::MoveJ>("rm_driver/MoveJ_Cmd", 10);

        //清除错误
        clear_pub =  nh.advertise<std_msgs::Empty>("rm_driver/Clear_System_Err", 10);

        //末端modbus rtu协议写入
        write_single_register_pub = nh.advertise<rm_msgs::write_single_register>("rm_driver/Write_Single_Register_Cmd",10);

        //初始化modbus RTU模式
        set_modbus_pub = nh.advertise<rm_msgs::set_modbus_mode>("rm_driver/Set_ModbusRTU_Cmd", 10);

        //设置末端电源输出
        toolpower_pub = nh.advertise<std_msgs::Byte>("rm_driver/SetToolVoltage",10);
        //初始化
        clearErr();
        //初始化末端
        init_tcp_motor();
        ros::Duration(1.0).sleep();
    }

    void moveJoint(std::vector<float> joint_angle_set,float speed)
    {
        //定义一个MoveJ指令的目标位姿
        rm_msgs::MoveJ moveJ_BeginPose;

        // moveJ_BeginPose.joint.resize(6);
        moveJ_BeginPose.joint[0] = joint_angle_set[0];
        moveJ_BeginPose.joint[1] = joint_angle_set[1];
        moveJ_BeginPose.joint[2] = joint_angle_set[2];
        moveJ_BeginPose.joint[3] = joint_angle_set[3];
        moveJ_BeginPose.joint[4] = joint_angle_set[4];
        moveJ_BeginPose.joint[5] = joint_angle_set[5];

        moveJ_BeginPose.speed = speed;
        //发布空间规划指令使机械臂运动到目标位姿
        moveJ_pub.publish(moveJ_BeginPose);
    }

    void veloMoveJoint(std::vector<float> joint_speed_vec, std::vector<float> current_joint_angle)
    {
        //定义一个MoveJ指令的目标位姿
        rm_msgs::MoveJ moveJ_BeginPose;
        // moveJ_BeginPose.joint.resize(6);
        moveJ_BeginPose.joint[0] = current_joint_angle[0];
        moveJ_BeginPose.joint[1] = current_joint_angle[1];
        moveJ_BeginPose.joint[2] = current_joint_angle[2];
        moveJ_BeginPose.joint[3] = current_joint_angle[3];
        moveJ_BeginPose.joint[4] = current_joint_angle[4];
        moveJ_BeginPose.joint[5] = current_joint_angle[5];

        for(int i=0;i<6;i++)
        {
            if(joint_speed_vec[i] > 0)
                moveJ_BeginPose.joint[i] += 0.087;
            else if(joint_speed_vec[i] < 0)
                moveJ_BeginPose.joint[i] -= 0.087;
        }
        moveJ_BeginPose.speed = 0.1;
        std::cout<<"target joint: "<<moveJ_BeginPose<<std::endl;
        //发布空间规划指令使机械臂运动到目标位姿
        moveJ_pub.publish(moveJ_BeginPose);
    }
    void clearErr()
    {
        std_msgs::Empty msg;
        clear_pub.publish(msg);
    }
    //开启机械臂tool接口电源
    void set_tool_power(uint8_t state)
    {
        std_msgs::Byte msg;
        msg.data = state;
        toolpower_pub.publish(msg);
    }
    void set_modbus()
    {
        //
        rm_msgs::set_modbus_mode msg;

        msg.port = 1;
        msg.baudrate = 9600;
        msg.timeout = 1;

        set_modbus_pub.publish(msg);
    }
    //设置电机正反转
    void set_tcp_motor_direction(bool forward)
    {
        rm_msgs::write_single_register msg;
        msg.port = 1;
        msg.address = 1;
        if(forward == true)
            msg.data = 1;
        else
            msg.data = 0;
        msg.device = 1;
        write_single_register_pub.publish(msg);
    }
    //设置电机运行
    void run_tcp_motor(bool run)
    {
        rm_msgs::write_single_register msg;
        msg.port = 1;
        msg.address = 2;
        if(run == true)
            msg.data = 1;
        else
            msg.data = 0;
        msg.device = 1;
        write_single_register_pub.publish(msg);
    }
    //停止电机
    void stop_tcp_motor()
    {
        rm_msgs::write_single_register msg;
        msg.port = 1;
        msg.address = 3;
        msg.data = 1;
        msg.device = 1;
        write_single_register_pub.publish(msg);
    }
    //设置tool电机速度
    void set_tcp_motor_speed(int16_t speed)
    {
        rm_msgs::write_single_register msg;
        msg.port = 1;
        msg.address = 4;
        msg.data = speed;
        msg.device = 1;
        write_single_register_pub.publish(msg);
    }
    //设置tool电机角度
    void set_tcp_motor_angle(int16_t angle)
    {
        rm_msgs::write_single_register msg;
        msg.port = 1;
        msg.address = 7;
        msg.data = angle;
        msg.device = 1;
        write_single_register_pub.publish(msg);
    }
    //初始化末端电机相关内容
    void init_tcp_motor()
    {
        //开启24V电源
        set_tool_power(3);
        //打开modbus模式
        set_modbus();
        //初始化电机
        set_tcp_motor_direction(true);
        set_tcp_motor_angle(0);
    }

    //运行电机
    void start_run_motor(bool if_forward, int16_t speed)
    {
        //先停止电机
        run_tcp_motor(false);
        ros::Duration(0.1).sleep();
        //设置电机方向
        set_tcp_motor_direction(if_forward);
        ros::Duration(0.1).sleep();
        //设置电机速度
        set_tcp_motor_speed(speed);
        ros::Duration(0.1).sleep();
        //开始运行
        run_tcp_motor(true);
    }
};